
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_rf_backend.h
  * @author     baiyang
  * @date       2023-2-9
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "sensor_rf_params.h"

#include <stdint.h>
#include <stdbool.h>

#include <rtthread.h>

#include <gcs_mavlink/gcs.h>
#include <common/gp_defines.h>
#include <common/gp_rotations.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#ifndef MB_RANGEFINDER_ENABLED
#define MB_RANGEFINDER_ENABLED RANGEFINDER_ENABLED
#endif

#ifndef MB_RANGEFINDER_BACKEND_DEFAULT_ENABLED
#define MB_RANGEFINDER_BACKEND_DEFAULT_ENABLED MB_RANGEFINDER_ENABLED
#endif

// Maximum number of range finder instances available on this platform
#ifndef RANGEFINDER_MAX_INSTANCES 
  #if MB_RANGEFINDER_ENABLED
  #define RANGEFINDER_MAX_INSTANCES 10
  #else
  #define RANGEFINDER_MAX_INSTANCES 1
  #endif
#endif

#define RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT 10
#define RANGEFINDER_PREARM_ALT_MAX_CM           200
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
#define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM   0
#else
#define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM   50
#endif
/*----------------------------------typedef-----------------------------------*/
// RangeFinder driver types
enum RangeFinderType {
    RANGEFINDER_NONE   = 0,
    RANGEFINDER_ANALOG = 1,
    RANGEFINDER_MBI2C  = 2,
    RANGEFINDER_PLI2C  = 3,
//  RANGEFINDER_PX4    = 4, // no longer used, but may be in some user's parameters
    RANGEFINDER_PX4_PWM= 5,
    RANGEFINDER_BBB_PRU= 6,
    RANGEFINDER_LWI2C  = 7,
    RANGEFINDER_LWSER  = 8,
    RANGEFINDER_BEBOP  = 9,
    RANGEFINDER_MAVLink = 10,
    RANGEFINDER_USD1_Serial = 11,
    RANGEFINDER_LEDDARONE = 12,
    RANGEFINDER_MBSER  = 13,
    RANGEFINDER_TRI2C  = 14,
    RANGEFINDER_PLI2CV3= 15,
    RANGEFINDER_VL53L0X = 16,
    RANGEFINDER_NMEA = 17,
    RANGEFINDER_WASP = 18,
    RANGEFINDER_BenewakeTF02 = 19,
    RANGEFINDER_BenewakeTFmini = 20,
    RANGEFINDER_PLI2CV3HP = 21,
    RANGEFINDER_PWM = 22,
    RANGEFINDER_BLPing = 23,
    RANGEFINDER_UAVCAN = 24,
    RANGEFINDER_BenewakeTFminiPlus = 25,
    RANGEFINDER_Lanbao = 26,
    RANGEFINDER_BenewakeTF03 = 27,
    RANGEFINDER_VL53L1X_Short = 28,
    RANGEFINDER_LeddarVu8_Serial = 29,
    RANGEFINDER_HC_SR04 = 30,
    RANGEFINDER_GYUS42v2 = 31,
    RANGEFINDER_MSP = 32,
    RANGEFINDER_USD1_CAN = 33,
    RANGEFINDER_Benewake_CAN = 34,
    RANGEFINDER_TeraRanger_Serial = 35,
    RANGEFINDER_SIM = 100,
};

enum RangeFinderFunction {
    RANGEFINDER_FUNC_LINEAR    = 0,
    RANGEFINDER_FUNC_INVERTED  = 1,
    RANGEFINDER_FUNC_HYPERBOLA = 2
};

enum RangeFinderStatus {
    RangeFinderNotConnected = 0,
    RangeFinderNoData,
    RangeFinderOutOfRangeLow,
    RangeFinderOutOfRangeHigh,
    RangeFinderGood
};

typedef struct RangeFinder_State* sensor_rf_state_t;

// The RangeFinder_State structure is filled in by the backend driver
struct RangeFinder_State {
    float distance_m;               // distance in meters
    uint16_t voltage_mv;            // voltage in millivolts, if applicable, otherwise 0
    enum RangeFinderStatus status; // sensor status
    uint8_t  range_valid_count;     // number of consecutive valid readings (maxes out at 10)
    uint32_t last_reading_ms;       // system time of last successful update from sensor
};

typedef struct sensor_rf_backend* sensor_rf_backend_t;
typedef const struct sensor_rf_backend* sensor_rf_backend_const_t;
typedef struct sensor_rf_backend_ops* sensor_rf_backend_ops_t;

struct sensor_rf_backend {
    sensor_rf_backend_ops_t ops;

    sensor_rf_state_t state;
    sensor_rf_params_t params;

    // mutex for access to shared frontend data
    rt_mutex_t _mutex;

    //Type Backend initialised with
    enum RangeFinderType _backend_type;
};

struct sensor_rf_backend_ops {
    void (*rf_backend_destructor)(sensor_rf_backend_t rf_backend);

    // update the state structure
    void (*update)(sensor_rf_backend_t rf_backend);

    void (*init_serial)(sensor_rf_backend_t rf_backend, uint8_t serial_instance);

    void (*handle_msg)(sensor_rf_backend_t rf_backend, const mavlink_message_t *msg);

    int16_t (*max_distance_cm)(sensor_rf_backend_const_t rf_backend);
    int16_t (*min_distance_cm)(sensor_rf_backend_const_t rf_backend);

    // get temperature reading in C.  returns true on success and populates temp argument
    bool (*get_temp)(sensor_rf_backend_const_t rf_backend, float *temp);

    // 0 is no return value, 100 is perfect.  false means signal
    // quality is not available
    bool (*get_signal_quality_pct)(sensor_rf_backend_const_t rf_backend, uint8_t *quality_pct);

    MAV_DISTANCE_SENSOR (*_get_mav_distance_sensor_type)(sensor_rf_backend_const_t rf_backend);
};

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_rf_backend_ctor(sensor_rf_backend_t rf_backend,
                                    sensor_rf_backend_ops_t _ops,
                                    sensor_rf_state_t _state, sensor_rf_params_t _params,
                                    const char *name);

static inline enum RotationEnum sensor_rf_orientation(sensor_rf_backend_const_t rf_backend) { return (enum RotationEnum)rf_backend->params->orientation; }
static inline float sensor_rf_distance(sensor_rf_backend_const_t rf_backend) { return rf_backend->state->distance_m; }
static inline uint16_t sensor_rf_distance_cm(sensor_rf_backend_const_t rf_backend) { return rf_backend->state->distance_m*100.0f; }
static inline uint16_t sensor_rf_voltage_mv(sensor_rf_backend_const_t rf_backend) { return rf_backend->state->voltage_mv; }
static inline int16_t sensor_rf_ground_clearance_cm(sensor_rf_backend_const_t rf_backend) { return rf_backend->params->ground_clearance_cm; }
MAV_DISTANCE_SENSOR sensor_rf_get_mav_distance_sensor_type(sensor_rf_backend_const_t rf_backend);
enum RangeFinderStatus sensor_rf_status(sensor_rf_backend_const_t rf_backend);
static inline enum RangeFinderType sensor_rf_type(sensor_rf_backend_const_t rf_backend) { return (enum RangeFinderType)rf_backend->params->type; }

// true if sensor is returning data
bool sensor_rf_has_data(sensor_rf_backend_const_t rf_backend);

// update status based on distance measurement
void sensor_rf_update_status(sensor_rf_backend_t rf_backend);

// set status and update valid_count
void sensor_rf_set_status(sensor_rf_backend_t rf_backend, enum RangeFinderStatus status);

// returns count of consecutive good readings
static inline uint8_t sensor_rf_range_valid_count(sensor_rf_backend_const_t rf_backend) { return rf_backend->state->range_valid_count; }

// return a 3D vector defining the position offset of the sensor
// in metres relative to the body frame origin
static inline const Vector3f_t *sensor_rf_get_pos_offset(sensor_rf_backend_const_t rf_backend) { return &rf_backend->params->pos_offset; }

// return system time of last successful read from the sensor
static inline uint32_t sensor_rf_last_reading_ms(sensor_rf_backend_const_t rf_backend) { return rf_backend->state->last_reading_ms; }

// return the actual type of the rangefinder, as opposed to the
// parameter value which may be changed at runtime.
static inline enum RangeFinderType sensor_rf_allocated_type(sensor_rf_backend_const_t rf_backend) { return rf_backend->_backend_type; }

/// 
void sensor_rf_backendDestructor(sensor_rf_backend_t rf_backend);

/// 
static inline void sensor_rf_destructor(sensor_rf_backend_t rf_backend) {
    if (rf_backend->ops->rf_backend_destructor != NULL) {
        rf_backend->ops->rf_backend_destructor(rf_backend);
        return;
    }

    sensor_rf_backendDestructor(rf_backend);
}

// update the state structure
static inline void sensor_rf_update(sensor_rf_backend_t rf_backend) {
    RT_ASSERT(rf_backend->ops->update != NULL);
    rf_backend->ops->update(rf_backend);
}

static inline void sensor_rf_init_serial(sensor_rf_backend_t rf_backend, uint8_t serial_instance) {
    if (rf_backend->ops->init_serial != NULL) {
        rf_backend->ops->init_serial(rf_backend, serial_instance);
    }
}

static inline void sensor_rf_handle_msg(sensor_rf_backend_t rf_backend, const mavlink_message_t *msg) {
    if (rf_backend->ops->handle_msg != NULL) {
        rf_backend->ops->handle_msg(rf_backend, msg);
    }
}

static inline int16_t sensor_rf_max_distance_cm(sensor_rf_backend_const_t rf_backend) {
    if (rf_backend->ops->max_distance_cm != NULL) {
        return rf_backend->ops->max_distance_cm(rf_backend);
    }

    return rf_backend->params->max_distance_cm;
}

static inline int16_t sensor_rf_min_distance_cm(sensor_rf_backend_const_t rf_backend) {
    if (rf_backend->ops->min_distance_cm != NULL) {
        return rf_backend->ops->min_distance_cm(rf_backend);
    }

    return rf_backend->params->min_distance_cm;
}

// get temperature reading in C.  returns true on success and populates temp argument
static inline bool sensor_rf_get_temp(sensor_rf_backend_const_t rf_backend, float *temp) {
    if (rf_backend->ops->get_temp != NULL) {
        return rf_backend->ops->get_temp(rf_backend, temp);
    }

    return false;
}

// 0 is no return value, 100 is perfect.  false means signal
// quality is not available
static inline bool sensor_rf_get_signal_quality_pct(sensor_rf_backend_const_t rf_backend, uint8_t *quality_pct) {
    if (rf_backend->ops->get_signal_quality_pct != NULL) {
        return rf_backend->ops->get_signal_quality_pct(rf_backend, quality_pct);
    }

    return false;
}
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



